#ifndef __BAG_FILE_H__
#define __BAG_FILE_H__

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <iostream>
#include <common_modules/file_manager/file_manager.h>
using namespace std;

class BagFile{
    public: 
        BagFile(){};
        ~BagFile(){};

        // 打开bag
        void BagOpen(string file_path){
            bagFilePath_ = file_path;
            try{
                if(file_manager::IsExists(file_path))
                    rosbag_.open(file_path, rosbag::bagmode::Append);       // 选择追加模式
                else
                    rosbag_.open(file_path, rosbag::bagmode::Write);        // 选择写入模式 

                // 设置压缩模式
                if(compressionMode_ == "BZ2"){
                    rosbag_.setCompression(rosbag::compression::BZ2);
                }
                else if(compressionMode_ == "LZ4"){
                    rosbag_.setCompression(rosbag::compression::LZ4);
                }
                else{
                    rosbag_.setCompression(rosbag::compression::Uncompressed);
                }
                
            }
            catch(rosbag::BagException& error){
                BagClose();
                cout << "[BagOpen] BagException: " << error.what() << endl;
                rosbag_.open(file_path, rosbag::bagmode::Write);        // 选择写入模式 
            }                             
            cout << "[BagOpen] bag file is_open: " << (rosbag_.isOpen() ? "true":"false") 
                << ", bagmode: " << (rosbag_.getMode()==rosbag::bagmode::Write ? "Write" : "Append") 
                << ", compression mode: " << compressionMode_
                << endl;            
        }

        // 关闭bag
        void BagClose(){
            if(rosbag_.isOpen()) rosbag_.close();
        }

        // 写入数据
        template <typename T>
        void BagWrite(const std::string& topic_name, const ros::Time& stamp, const T& msg){
            rosbag_.write(topic_name, stamp, msg);
            // ROS_INFO("Written message to topic: %s", topic_name.c_str());
        }

    private:
        string bagFilePath_;            //当前BAG文件路径
        rosbag::Bag rosbag_;            //rosbag
        string compressionMode_;        //压缩模式
};

#endif